#include "socket_manager.h"

using namespace Communication;

SocketManger::SocketManger(ros::NodeHandle nh, ros::NodeHandle private_nh, const std::string ip, const unsigned short port)
: SocketClientInterface(ip, port),
  nh_(nh),
  private_nh_(private_nh),
  time_start_connect_(0.0f),
  recv_buffer_(10),
  send_buffer_(10)
{
    ctrl_velocity_.mode = eMoveMode::HEADFOLLOW;
    ctrl_velocity_.speed = 0.5f;
    ctrl_velocity_.omega = 0.0f;
    ctrl_velocity_.speed_toward = 0.0f;
    
    ptr_actuator_ = (std::make_unique<actuator::ActuatorMain>(nh_, private_nh_, param, log));
    ptr_task_process_ = std::make_unique<task::TaskProcess>(nh_, private_nh_);

    struct SteerParams param;
    param.pos_x = 0.5f;
    param.pos_y = 0.0f;
    amr_state_.steer_param.emplace_back(param);
    param.pos_x = -0.5f;
    param.pos_y = 0.0f;
    amr_state_.steer_param.emplace_back(param);

    server_is_connected_ = true;
}

SocketManger::~SocketManger()
{
    
}

void SocketManger::Start(void)
{   
    ros::Rate loop(1);
    while (ros::ok())
    {
        if (server_is_connected_)
        {
            TestRecvBuffer();
            RecvDataHandle();
        }
        else
        {
            if ((ros::Time::now() - time_start_connect_).toSec() > 3.0f)
            {
                AsyncConnect();
                time_start_connect_ = ros::Time::now();
            }
        }

        loop.sleep();
        ros::spinOnce();
    }
}

void SocketManger::OnConnectStatus(bool *isConnected, boost::system::error_code *ec) 
{
    printf("SocketManger Connect Callback() --> %d, %s\n", *isConnected, ec->message().c_str());
    server_is_connected_ = isConnected;
}

void SocketManger::ReadCallback(std::string *str_read, boost::system::error_code *ec)
{
    printf("SocketManger Read Callback() --> %s, %s\n", (*str_read).c_str(), ec->message().c_str());
    recv_buffer_.Push(*str_read);
}

void SocketManger::HandleWrite(std::size_t *length, boost::system::error_code *ec) 
{
    printf("SocketManger Write Callback() --> %ld, %s\n", *length, ec->message().c_str());
}

void SocketManger::GetServoParams(std::string &param_file)
{
    while (send_buffer_.is_not_empty())
    {
        std::string s;
        send_buffer_.Pop(&s);

        // Send
        WriteSome(s);
    }
}

void SocketManger::TestRecvBuffer()
{
    Json::Value message;
#if 1
    Json::Value servo;
    servo["id"] = 1;
    servo["speed"] = 0.0f;
    servo["angle"] = 0.0f;
    message["servo"].append(servo);
    servo["id"] = 2;
    servo["speed"] = 0.0f;
    servo["angle"] = 0.0f;
    message["servo"].append(servo);
    std::string cmd = "periodic_sensor";
    message["command"] = cmd;
#else
    Json::Value edge;
    edge["edge"] = 1;
    edge["max_speed"] = 1.0f;
    message["edges"].append(edge);
    edge["edge"] = 2;
    edge["max_speed"] = 1.0f;
    message["edges"].append(edge);
    edge["edge"] = 3;
    edge["max_speed"] = 1.0f;
    message["edges"].append(edge);
    std::string cmd = "move";
    message["sn"] = 100L;
    message["command"] = cmd;
    message["target_id"] = "4";
    message["is_instantly_stop"] = 0;

#endif
    Json::FastWriter writer;
    std::string json_file = writer.write(message);
    std::cout << json_file << std::endl;

    recv_buffer_.Push(json_file);
}

void SocketManger::RecvDataHandle(void)
{
    while (recv_buffer_.is_not_empty())
    {
        std::string recv;
        recv_buffer_.Pop(&recv);

        const auto rawJsonLength = static_cast<int>(recv.length());
        JSONCPP_STRING err;
        Json::Value root;
        Json::CharReaderBuilder builder;
        const std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
        if (!reader->parse(recv.c_str(), recv.c_str() + rawJsonLength, &root, &err))
        {
            std::cout << "error: " << err.c_str() << std::endl;
            continue;
        }
        
        std::string cmd = root["command"].asString();

        if (cmd.compare("periodic_sensor") == 0) // 反馈数据解析
        {
            ServoFeedbackParse(root);
        }
        else // 调度任务解析
        {
            std::string respond = TaskCmdParse(root);
            send_buffer_.Push(respond);
        }
    }   
}

void SocketManger::SendDataHandle(void)
{
    
}

std::string SocketManger::JsonWriter(Json::Value &value)
{
    Json::FastWriter writer;
    std::string json_file = writer.write(value);
    std::cout << json_file << std::endl;
    return json_file;
}

void SocketManger::ServoFeedbackParse(Json::Value &value)
{
    if (value["servo"].isNull())
        return;
    ros::Time start = ros::Time::now();
    Json::Value servo = value["servo"];

    printf("servo size: %d\n", servo.size());
    amr_state_.steer_state.clear();
    amr_state_.steer_state.resize(amr_state_.steer_param.size());

    for (int i = 0;i < servo.size();i++)
    {
        struct SteerState state;
        int id = servo[i]["id"].asInt();
        state.steer_angle = servo[i]["angle"].asFloat();
        state.steer_speed = servo[i]["speed"].asFloat();

        // printf("id: %d, v: %f, angle: %f\n", id, state.steer_speed, state.steer_angle);

        amr_state_.steer_state[id-1] = state;
    }
    
    ptr_actuator_->ActuatorRecvHandler(amr_cmd_, amr_state_, vehicle_velocity_);
    ptr_actuator_->ActuatorSendHandler(amr_cmd_, amr_state_, ctrl_velocity_);
    printf("dtime: %f\n", (ros::Time::now()-start).toSec());
    Json::Value period_response;
    period_response["sn"] = value["sn"].asUInt64();
}

std::string SocketManger::TaskCmdParse(Json::Value &task_cmd)
{
    std::string ret("");
    std::string cmd = task_cmd["command"].asString();
    if (cmd.compare("set") == 0)
    {   
        std::string map_file = task_cmd["map_id"].asString();
        ptr_task_process_->SetMapFile(map_file);

        // Write Json Result
        Json::Value confirm;
        confirm["sn"] = task_cmd["sn"].asUInt64();
        confirm["headers"] = "confirm_set";
        confirm["respond"] = "YES";
        ret = JsonWriter(confirm);
    }
    else if (cmd.compare("get") == 0)
    {
        std::vector<task::sFirmwareInfo> tmp;
        ptr_task_process_->GetFirmwareInfo(tmp);

        // Write Json Result

        Json::Value confirm;
        confirm["sn"] = task_cmd["sn"].asUInt64();
        confirm["headers"] = "confirm_get";
        confirm["respond"] = "YES";
        ret = JsonWriter(confirm);
    }
    else if (cmd.compare("move") == 0)
    {
        task::sTaskMsg task_msg;
        Json::Value edgeinfo = task_cmd["edges"];
        printf("edge size: %d\n", edgeinfo.size());

        for (int i=0;i<edgeinfo.size();i++)
        {
            task::sTaskPath path;
            path.path_id = edgeinfo[i]["edge"].asString();
            path.max_speed = edgeinfo[i]["max_speed"].asFloat();
            printf("dege: %s, max_speed: %f\n", path.path_id.c_str(), path.max_speed);
            task_msg.v_task_path.push_back(path);
        }

        task_msg.target_point_id = task_cmd["target_id"].asString();
        printf("target point: %s\n", task_msg.target_point_id.c_str());

        ptr_task_process_->TaskInteract(task_msg);

        Json::Value confirm;
        confirm["sn"] = task_cmd["sn"].asUInt64();
        confirm["headers"] = "confirm_move";
        confirm["respond"] = "YES";
        ret = JsonWriter(confirm);
    }
    return ret;
}